Fast Marching Trees

In this paper we present a novel probabilistic sampling-based motion planning algorithm called the Fast Marching Tree algorithm (FMT∗ ). The algorithm is specifically aimed at solving complex motion planning problems in highdimensional configuration spaces. This algorithm is proven to be asymptotically optimal and is shown to converge to an optimal solution faster than its state-of-the-art counterparts, chiefly PRM* and RRT*. An additional advantage of FMT∗ is that it builds and maintains paths in a tree-like structure (especially useful for planning under differential constraints). The FMT∗ algorithm essentially performs a “lazy” dynamic programming recursion on a set of probabilistically-drawn samples to grow a tree of paths, which moves steadily outward in cost-to-come space. As such, this algorithm combines features of both single-query algorithms (chiefly RRT) and multiple-query algorithms (chiefly PRM), and is conceptually related to the Fast Marching Method for the solution of eikonal equations. As a departure from previous analysis approaches that are based on the notion of almost sure convergence, the FMT∗ algorithm is analyzed under the notion of convergence in probability: the extra mathematical flexibility of this approach allows for significant algorithmic advantages and provides convergence rate bounds – a first in the field of optimal sampling-based motion planning. Numerical experiments over a range of dimensions and obstacle configurations confirm our theoretical and heuristic arguments by showing that FMT∗ , for a given execution time, returns substantially better solutions than either PRM* or RRT*, especially in high-dimensional configuration spaces. Lucas Janson Department of Statistics, Stanford University, Stanford, CA 94305, e-mail: [email protected] Marco Pavone Department of Aeronautics and Astronautics, Stanford University, Stanford, CA 94305, e-mail: [email protected] A preliminary version of this work has been orally presented at the workshop on “Robotic Exploration, Monitoring, and Information Collection: Nonparametric Modeling, Information-based Control, and Planning under Uncertainty” at the Robotics: Science and Systems 2013 conference. This work has neither appeared elsewhere for publication, nor is under review for another refereed publication.